#include<ros/ros.h>
#include <atomic>

#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<std_msgs/UInt64.h>

#include "unionsys_core.hpp"

#include"../SDPltUtils/SDPltBase.hpp"
#include "../SDPltDDS/SDPltPublisher.hpp"
#include "../SDPltDDS/SDPltSubscriber.hpp"

#include "../SDPltDDSPose/SDPltPoseMsg.h"
#include "../SDPltDDSPose/SDPltPoseMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltVelocityMsg.h"
#include "../SDPltDDSPose/SDPltVelocityMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltStateMsg.h"
#include "../SDPltDDSPose/SDPltStateMsgPubSubTypes.h"
#include "../SDPltDDSPose/pointcloud2.h"
#include "../SDPltDDSPose/pointcloud2PubSubTypes.h"


//接收dds消息，并发布到ros节点


RegisterMSN *reg_sub = new RegisterMSN();

int self_drone_id;

//声明fastdds订阅、ros的发布类
SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddssub_pose1=nullptr;
ros::Publisher rospub_pose1; 
SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddssub_pose2=nullptr;
ros::Publisher rospub_pose2; 
SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddssub_pose3=nullptr;
ros::Publisher rospub_pose3; 
SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddssub_pose4=nullptr;
ros::Publisher rospub_pose4; 
SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddssub_pose5=nullptr;
ros::Publisher rospub_pose5; 

SDPltSubscriber<SDPltVelocityMsgPubSubType, SDPltVelocityMsg>* ddssub_velocity=nullptr;
ros::Publisher rospub_velocity; 

SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddssub_goal=nullptr;
ros::Publisher rospub_goal; 


//fastdds(pose)回调函数
void ddsposeloopdata1(SDPltPoseMsg* pose){    

    geometry_msgs::PoseStamped current_pose;
    
    //赋值
    current_pose.header.seq = pose->seq_();

    current_pose.header.frame_id = pose->frame_id_();

    current_pose.pose.position.x = pose->xPoint_();
    current_pose.pose.position.y = pose->yPoint_();
    current_pose.pose.position.z = pose->zPoint_();
    current_pose.pose.orientation.x = pose->xOrientation_();
    current_pose.pose.orientation.y = pose->yOrientation_();
    current_pose.pose.orientation.z = pose->zOrientation_();
    current_pose.pose.orientation.w = pose->wOrientation_();

    rospub_pose1.publish(current_pose);

}
void ddsposeloopdata2(SDPltPoseMsg* pose){    

    geometry_msgs::PoseStamped current_pose;
    
    //赋值
    current_pose.header.seq = pose->seq_();

    current_pose.header.frame_id = pose->frame_id_();

    current_pose.pose.position.x = pose->xPoint_();
    current_pose.pose.position.y = pose->yPoint_();
    current_pose.pose.position.z = pose->zPoint_();
    current_pose.pose.orientation.x = pose->xOrientation_();
    current_pose.pose.orientation.y = pose->yOrientation_();
    current_pose.pose.orientation.z = pose->zOrientation_();
    current_pose.pose.orientation.w = pose->wOrientation_();

    rospub_pose2.publish(current_pose);

}
void ddsposeloopdata3(SDPltPoseMsg* pose){    

    geometry_msgs::PoseStamped current_pose;
    
    //赋值
    current_pose.header.seq = pose->seq_();

    current_pose.header.frame_id = pose->frame_id_();

    current_pose.pose.position.x = pose->xPoint_();
    current_pose.pose.position.y = pose->yPoint_();
    current_pose.pose.position.z = pose->zPoint_();
    current_pose.pose.orientation.x = pose->xOrientation_();
    current_pose.pose.orientation.y = pose->yOrientation_();
    current_pose.pose.orientation.z = pose->zOrientation_();
    current_pose.pose.orientation.w = pose->wOrientation_();

    rospub_pose3.publish(current_pose);

}
void ddsposeloopdata4(SDPltPoseMsg* pose){    

    geometry_msgs::PoseStamped current_pose;
    
    //赋值
    current_pose.header.seq = pose->seq_();

    current_pose.header.frame_id = pose->frame_id_();

    current_pose.pose.position.x = pose->xPoint_();
    current_pose.pose.position.y = pose->yPoint_();
    current_pose.pose.position.z = pose->zPoint_();
    current_pose.pose.orientation.x = pose->xOrientation_();
    current_pose.pose.orientation.y = pose->yOrientation_();
    current_pose.pose.orientation.z = pose->zOrientation_();
    current_pose.pose.orientation.w = pose->wOrientation_();

    rospub_pose4.publish(current_pose);

}
void ddsposeloopdata5(SDPltPoseMsg* pose){    

    geometry_msgs::PoseStamped current_pose;
    
    //赋值
    current_pose.header.seq = pose->seq_();

    current_pose.header.frame_id = pose->frame_id_();

    current_pose.pose.position.x = pose->xPoint_();
    current_pose.pose.position.y = pose->yPoint_();
    current_pose.pose.position.z = pose->zPoint_();
    current_pose.pose.orientation.x = pose->xOrientation_();
    current_pose.pose.orientation.y = pose->yOrientation_();
    current_pose.pose.orientation.z = pose->zOrientation_();
    current_pose.pose.orientation.w = pose->wOrientation_();

    rospub_pose5.publish(current_pose);

}

void dds_goal_cb(SDPltPoseMsg* pose){    

    geometry_msgs::PoseStamped current_pose;
    
    //赋值
    current_pose.header.seq = pose->seq_();
    std::vector<std::string> res_temp;
    res_temp = reg_sub->deserial_str(pose->frame_id_());
    int select_self = atoi(res_temp[self_drone_id-1].c_str());
    
    current_pose.header.frame_id = "map";

    current_pose.pose.position.x = pose->xPoint_();
    current_pose.pose.position.y = pose->yPoint_();
    current_pose.pose.position.z = pose->zPoint_();
    current_pose.pose.orientation.x = pose->xOrientation_();
    current_pose.pose.orientation.y = pose->yOrientation_();
    current_pose.pose.orientation.z = pose->zOrientation_();
    current_pose.pose.orientation.w = pose->wOrientation_();

    if(select_self==1)
    {
        rospub_goal.publish(current_pose);
    }
    

}


//fastdds(velocity)回调函数
void ddsvelocityloopdata(SDPltVelocityMsg* velocity){    

    geometry_msgs::TwistStamped current_velocity;
    //赋值
    current_velocity.header.seq = velocity->seq_();
    current_velocity.header.frame_id = velocity->frame_id_();
    current_velocity.twist.linear.x = velocity->xlinear_();
    current_velocity.twist.linear.y = velocity->ylinear_();
    current_velocity.twist.linear.z = velocity->zlinear_();
    current_velocity.twist.angular.x = velocity->xangular_();
    current_velocity.twist.angular.y = velocity->yangular_();
    current_velocity.twist.angular.z = velocity->zangular_();

    rospub_velocity.publish(current_velocity);

}


int main(int argc, char* argv[]){
    reg_sub->detect_serial_number();
    ros::init(argc, argv, "SDPltDDSPosesub");
    ros::NodeHandle nodeHandle; 

    // 设置循环的频率
    ros::Rate loop_rate(30);

    //从SDPltDDSPose.launch文件中获取dds、ros发布订阅topic的名字
    std::string dds_sub_Posetopic_name1, dds_sub_Posetopic_name2, dds_sub_Posetopic_name3, dds_sub_Posetopic_name4, dds_sub_Posetopic_name5;
    std::string ros_pub_Posetopic_name1, ros_pub_Posetopic_name2, ros_pub_Posetopic_name3, ros_pub_Posetopic_name4, ros_pub_Posetopic_name5;
    std::string  ros_pub_Velocitytopic_name, dds_sub_Velocitytopic_name;
    nodeHandle.getParam("drone_id",self_drone_id);
    nodeHandle.getParam("ros_pub_Posetopic_name1",ros_pub_Posetopic_name1);
    nodeHandle.getParam("ros_pub_Posetopic_name2",ros_pub_Posetopic_name2);
    nodeHandle.getParam("ros_pub_Posetopic_name3",ros_pub_Posetopic_name3);
    nodeHandle.getParam("ros_pub_Posetopic_name4",ros_pub_Posetopic_name4);
    nodeHandle.getParam("ros_pub_Posetopic_name5",ros_pub_Posetopic_name5);

    nodeHandle.getParam("ros_pub_Velocitytopic_name",ros_pub_Velocitytopic_name);

    nodeHandle.getParam("dds_sub_Posetopic_name1",dds_sub_Posetopic_name1);
    nodeHandle.getParam("dds_sub_Posetopic_name2",dds_sub_Posetopic_name2);
    nodeHandle.getParam("dds_sub_Posetopic_name3",dds_sub_Posetopic_name3);
    nodeHandle.getParam("dds_sub_Posetopic_name4",dds_sub_Posetopic_name4);
    nodeHandle.getParam("dds_sub_Posetopic_name5",dds_sub_Posetopic_name5);

    nodeHandle.getParam("dds_sub_Velocitytopic_name",dds_sub_Velocitytopic_name);


    //dds先接收topic，后ros再发布topic
    ddssub_pose1 = new SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>(dds_sub_Posetopic_name1,"SDPltPoseMsg",ddsposeloopdata1,BEST_EFFORT_RELIABILITY_QOS);
    rospub_pose1 = nodeHandle.advertise<geometry_msgs::PoseStamped>(ros_pub_Posetopic_name1.c_str(),10);
    ddssub_pose2 = new SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>(dds_sub_Posetopic_name2,"SDPltPoseMsg",ddsposeloopdata2,BEST_EFFORT_RELIABILITY_QOS);
    rospub_pose2 = nodeHandle.advertise<geometry_msgs::PoseStamped>(ros_pub_Posetopic_name2.c_str(),10);
    ddssub_pose3 = new SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>(dds_sub_Posetopic_name3,"SDPltPoseMsg",ddsposeloopdata3,BEST_EFFORT_RELIABILITY_QOS);
    rospub_pose3 = nodeHandle.advertise<geometry_msgs::PoseStamped>(ros_pub_Posetopic_name3.c_str(),10);
    ddssub_pose4 = new SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>(dds_sub_Posetopic_name4,"SDPltPoseMsg",ddsposeloopdata4,BEST_EFFORT_RELIABILITY_QOS);
    rospub_pose4 = nodeHandle.advertise<geometry_msgs::PoseStamped>(ros_pub_Posetopic_name4.c_str(),10);
    ddssub_pose5 = new SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>(dds_sub_Posetopic_name5,"SDPltPoseMsg",ddsposeloopdata5,BEST_EFFORT_RELIABILITY_QOS);
    rospub_pose5 = nodeHandle.advertise<geometry_msgs::PoseStamped>(ros_pub_Posetopic_name5.c_str(),10);

    ddssub_goal = new SDPltSubscriber<SDPltPoseMsgPubSubType, SDPltPoseMsg>("ddsGoaltopic","SDPltPoseMsg",dds_goal_cb,BEST_EFFORT_RELIABILITY_QOS);
    rospub_goal = nodeHandle.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",10);


    ddssub_velocity = new SDPltSubscriber<SDPltVelocityMsgPubSubType, SDPltVelocityMsg>(dds_sub_Velocitytopic_name,"SDPltVelocityMsg",ddsvelocityloopdata,BEST_EFFORT_RELIABILITY_QOS);
    rospub_velocity = nodeHandle.advertise<geometry_msgs::TwistStamped>(ros_pub_Velocitytopic_name.c_str(),10);
    //循环回调
    while(ros::ok()&&nodeHandle.ok()){        
        ros::spinOnce();
        loop_rate.sleep();
    }

    nodeHandle.shutdown();
    return 0;
}
